{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "562059cb-a607-4b25-95c3-3f1f601ac569",
   "metadata": {},
   "source": [
    "# 手眼标定\n",
    "\n",
    "同济子豪兄 2024-5-13\n",
    "\n",
    "## 手眼标定的原理\n",
    "\n",
    "已知图像上两个点的【像素坐标】和【机械臂坐标】，就可以通过【线性插值】，建立像素坐标到机械臂坐标的映射关系。输入图像任意一点的像素坐标，eye2hand函数就能转换为机械臂坐标。让机械臂移动到图像上的同一个位置。\n",
    "\n",
    "在utils_robot.py里修改eye2hand函数，填好八个数字坐标。"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ac9fcc38-af22-4667-b1d1-a7408af011b1",
   "metadata": {},
   "source": [
    "## 导入工具包"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "e2060a44-ece4-4b3b-abc4-6096be080223",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pymycobot.mycobot import MyCobot\n",
    "from pymycobot import PI_PORT, PI_BAUD\n",
    "import time\n",
    "\n",
    "import cv2\n",
    "import numpy as np\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4d36547f-06a3-423a-91a8-f1d84bdd6dae",
   "metadata": {},
   "source": [
    "## 连接机械臂"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "5e665862-db0e-42a9-b26b-2cdbf614b5ac",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc = MyCobot(PI_PORT, PI_BAUD)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e5f4534f-13cc-43a2-b7b4-15d0bd5d3031",
   "metadata": {},
   "source": [
    "## 设置运动模式为插补"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "6e9cf1b3-2d0f-4e6e-8174-62e7113b787b",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.set_fresh_mode(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fc66a1ad-67bd-43ff-aa79-f011255b0bcd",
   "metadata": {},
   "source": [
    "## 机械臂归零"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "bd804f52-710b-4003-9a84-8d0355bff012",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_angles([0, 0, 0, 0, 0, 0], 40)\n",
    "time.sleep(3)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "4d62707b-f9d3-4934-99cf-107074a4553c",
   "metadata": {},
   "source": [
    "## 第一步：移动至俯视姿态\n",
    "\n",
    "俯视姿态一（关节）：[0, 0, -88, 0, 0, 45]\n",
    "\n",
    "俯视姿态二（坐标）：[13, -160, 212, 180, 3.31, -135.81]\n",
    "\n",
    "俯视姿态二（关节）：[-62.13, 8.96, -87.71, -14.41, 2.54, -16.34]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "964562be-f96c-49d2-a075-e6121035cc4c",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_coords([13, -160, 212, 180, 3.31, -135.81], 10)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "0d31eb93-ff2f-45a1-ac0e-1b3fcfa167c1",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-62.13, 8.26, -86.57, -14.32, 2.81, -16.52]"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mc.get_angles()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "53731c82-dde4-4cf6-a30c-409b49050423",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_angles([-62.13, 8.96, -87.71, -14.41, 2.54, -16.34], 10)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ebadf824-9b6e-4b1a-9b2a-36a6a485a517",
   "metadata": {},
   "source": [
    "## 第二步：在白纸靠左下角的位置，画一个标定点\n",
    "\n",
    "## 第三步：运行check_camera.py，打开摄像头实时画面\n",
    "\n",
    "## 第四步：把白纸右上角对准画面右上角\n",
    "\n",
    "## 第五步：白纸上边与底座、图像上边平齐，白纸下边与图像下边平齐\n",
    "\n",
    "## 第六步：用夹子固定白纸，分别夹左上角和右下角。（把麦克风线也固定）\n",
    "\n",
    "## 第七步：通过鼠标点选，获取白纸左下角标定点，在图像上的像素坐标"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "02a21378-1574-414d-a878-53906d21d663",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 第一个标定点的像素坐标\n",
    "cali_1_im = [130, 290]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1ded7260-bcba-4e61-8dc4-570a54034e9d",
   "metadata": {},
   "source": [
    "## 第八步：控制机械臂，移动至左下角第一个标定点"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "56dd19d8-c8d0-4b62-b603-34b9f3b83730",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 移动到标定点附近\n",
    "mc.send_coords([-36, -205, 94.7, 178.92, 4.56, -135.57], 10)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "bc1c49e3-6a9b-4380-86d2-30c28538d89f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-34.1, -205.0, 92.8, 178.45, 4.98, -135.44]"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mc.get_coords()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "11a5d770-3f66-4e2b-b800-43cc5ebe01f0",
   "metadata": {},
   "outputs": [],
   "source": [
    "X = -36"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "b7d73aa4-5851-4076-8eb2-0a3d7eb5f5fe",
   "metadata": {},
   "outputs": [],
   "source": [
    "X += 15\n",
    "mc.send_coord(1, X, 20)\n",
    "time.sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "89aa31af-cbf2-4956-963e-4b52c963ad22",
   "metadata": {},
   "outputs": [],
   "source": [
    "Y = -205"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "9ba5c4af-d773-4502-be4f-389b668434b8",
   "metadata": {},
   "outputs": [],
   "source": [
    "Y -= 8\n",
    "mc.send_coord(2, Y, 20)\n",
    "time.sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "aa7273a2-f71f-4f18-b506-2e569a528a52",
   "metadata": {},
   "outputs": [],
   "source": [
    "Z = 95"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "c1d972c3-e0ad-42b1-8d79-732ca3f46698",
   "metadata": {},
   "outputs": [],
   "source": [
    "Z -= 10\n",
    "mc.send_coord(3, Z, 20)\n",
    "time.sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "id": "272219bc-a392-48a3-919f-ecb2b2e2df83",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-21.8, -197.4, 59.3, 177.67, 5.2, -136.02]"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mc.get_coords()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b8b8e64e-15e9-413d-a7a5-5516eb4f27a0",
   "metadata": {},
   "source": [
    "- 记下机械臂坐标"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "id": "3a74a39c-0d8f-4eab-8dba-210a14da941d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 第一个标定点的机械臂坐标\n",
    "cali_1_mc = [-21.8, -197.4]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7a19cf5b-0475-48f0-8369-d3622e3a47d9",
   "metadata": {},
   "source": [
    "## 第九步：控制机械臂，移动至右上角第二个标定点"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "id": "302660c2-197d-440c-b5cd-cbad116cc823",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 第二个标定点的像素坐标\n",
    "cali_2_im = [640, 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "id": "1bb11271-3823-4089-8e61-63e9428dfe65",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 机械臂归零\n",
    "mc.send_angles([0, 0, 0, 0, 0, 0], 40)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "id": "31c0630f-7620-4006-930d-fc15f6355423",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 移动到第二个标定点附近\n",
    "mc.send_coords([211, -62, 120, 180, 3.31, -135.81], 10)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 245,
   "id": "490e1c10-14f6-4e76-b443-a5b1a14ebe9b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[188.4, -49.8, 141.4, 178.41, 1.11, -135.72]"
      ]
     },
     "execution_count": 245,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mc.get_coords()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "id": "d9207878-aab9-47e1-805c-570a050d0bef",
   "metadata": {},
   "outputs": [],
   "source": [
    "Z = 120"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 70,
   "id": "0baab154-29c0-4bcc-bb86-09faf443f190",
   "metadata": {},
   "outputs": [],
   "source": [
    "Z -= 15\n",
    "mc.send_coord(3, Z, 20)\n",
    "time.sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "id": "c9938a50-45ff-45ed-b921-4e2a7cdc410f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[211.3, -60.8, 92.8, 178.55, 2.69, -136.32]"
      ]
     },
     "execution_count": 54,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mc.get_coords()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "id": "938e7cdd-1eb4-4614-bfe9-90bf4a91e5ac",
   "metadata": {},
   "outputs": [],
   "source": [
    "X = 211"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "id": "f626d705-b961-4ac0-9ccb-66f504a90b57",
   "metadata": {},
   "outputs": [],
   "source": [
    "X += 5\n",
    "mc.send_coord(1, X, 20)\n",
    "time.sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "id": "caeff852-c1e6-439b-9470-5f4302d7ff3f",
   "metadata": {},
   "outputs": [],
   "source": [
    "Y = -62"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 62,
   "id": "c7c9a855-b2f1-482e-9b6b-094620e01392",
   "metadata": {},
   "outputs": [],
   "source": [
    "Y += 5 \n",
    "mc.send_coord(2, Y, 20)\n",
    "time.sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 71,
   "id": "71505d9b-1cd5-4aed-bd95-1944bb39be9b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[215.1, -59.1, 75.8, 178.6, 2.24, -136.5]"
      ]
     },
     "execution_count": 71,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mc.get_coords()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 72,
   "id": "b6042efb-9534-49af-b71f-65d1fa3d0473",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 第二个标定点的机械臂坐标\n",
    "cali_2_mc = [215, -59.1]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f7d0bce1-225d-4f90-a7cb-72bfe2255e3b",
   "metadata": {},
   "source": [
    "## 第十步：通过插值，获取图像任意像素坐标对应的机械臂坐标"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 73,
   "id": "7fb7057b-0936-41fb-8034-87d686cbcc48",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 整理两个标定点的坐标\n",
    "cali_1_im = [130, 290]                       # 左下角，第一个标定点的像素坐标，要手动填！\n",
    "cali_1_mc = [-21.8, -197.4]                  # 左下角，第一个标定点的机械臂坐标，要手动填！\n",
    "cali_2_im = [640, 0]                         # 右上角，第二个标定点的像素坐标\n",
    "cali_2_mc = [215, -59.1]                    # 右上角，第二个标定点的机械臂坐标，要手动填！"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 74,
   "id": "61655532-e4b0-4c56-84f8-ca3704d7308e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 指定点在图像中的像素坐标\n",
    "X_im = 320\n",
    "Y_im = 240"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 75,
   "id": "4c32b19d-995c-4433-9670-043f8b7613f1",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_cali_im = [cali_1_im[0], cali_2_im[0]]     # 像素坐标\n",
    "X_cali_mc = [cali_1_mc[0], cali_2_mc[0]]     # 机械臂坐标\n",
    "\n",
    "X_mc = int(np.interp(X_im, X_cali_im, X_cali_mc))\n",
    "\n",
    "Y_cali_im = [cali_2_im[1], cali_1_im[1]]     # 像素坐标，先小后大\n",
    "Y_cali_mc = [cali_2_mc[1], cali_1_mc[1]]     # 机械臂坐标，先大后小\n",
    "\n",
    "Y_mc = int(np.interp(Y_im, Y_cali_im, Y_cali_mc))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 76,
   "id": "96d1501f-a689-42f2-b54c-1d54b3437010",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "66"
      ]
     },
     "execution_count": 76,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "X_mc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 77,
   "id": "3af491e2-8c7e-40ef-bf12-25d6f4fe1cb7",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-173"
      ]
     },
     "execution_count": 77,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Y_mc"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bc995043-f07f-47df-b9ae-3120b5bf4253",
   "metadata": {},
   "source": [
    "## 让机械臂移动至该点吸取"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 78,
   "id": "9eddbc0c-3247-412d-b291-0c9c704d2dd3",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_coords([X_mc, Y_mc, 200, -178.24, 1.68, -134.33], 20)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 86,
   "id": "fb08a954-ee74-4db9-8504-c5cead7f371a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 机械臂归零\n",
    "mc.send_angles([0, 0, 0, 0, 0, 0], 40)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5c86bde9-379b-42ed-bb1e-c1271b148427",
   "metadata": {},
   "source": [
    "## 封装手眼标定函数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 79,
   "id": "057a98f6-7207-438b-b6fc-dfdff480c481",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "def eye2hand(X_im=160, Y_im=120):\n",
    "    '''\n",
    "    输入目标点在图像中的像素坐标，转换为机械臂坐标\n",
    "    '''\n",
    "\n",
    "    # 整理两个标定点的坐标\n",
    "    cali_1_im = [130, 290]                       # 左下角，第一个标定点的像素坐标，要手动填！\n",
    "    cali_1_mc = [-21.8, -197.4]                  # 左下角，第一个标定点的机械臂坐标，要手动填！\n",
    "    cali_2_im = [640, 0]                         # 右上角，第二个标定点的像素坐标\n",
    "    cali_2_mc = [215, -59.1]                    # 右上角，第二个标定点的机械臂坐标，要手动填！\n",
    "    \n",
    "    X_cali_im = [cali_1_im[0], cali_2_im[0]]     # 像素坐标\n",
    "    X_cali_mc = [cali_1_mc[0], cali_2_mc[0]]     # 机械臂坐标\n",
    "    Y_cali_im = [cali_2_im[1], cali_1_im[1]]     # 像素坐标，先小后大\n",
    "    Y_cali_mc = [cali_2_mc[1], cali_1_mc[1]]     # 机械臂坐标，先大后小\n",
    "\n",
    "    # X差值\n",
    "    X_mc = int(np.interp(X_im, X_cali_im, X_cali_mc))\n",
    "\n",
    "    # Y差值\n",
    "    Y_mc = int(np.interp(Y_im, Y_cali_im, Y_cali_mc))\n",
    "\n",
    "    return X_mc, Y_mc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 80,
   "id": "c6810bb1-2455-41b5-aad3-62967f67dc75",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(215, -59)"
      ]
     },
     "execution_count": 80,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "eye2hand(X_im=640, Y_im=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 81,
   "id": "777a657f-1ab6-4439-85da-62ad1b0f79df",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(66, -173)"
      ]
     },
     "execution_count": 81,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "eye2hand(X_im=320, Y_im=240)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "55d96721-b3ce-4894-bb26-cc7b021c0ddc",
   "metadata": {},
   "source": [
    "## 第十一步：验证标定效果"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 82,
   "id": "e343e943-933b-43d1-9e41-00afcc59f9a3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 机械臂归零\n",
    "mc.send_angles([0, 0, 0, 0, 0, 0], 40)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 83,
   "id": "54c57f03-9af3-4124-9d21-0d363dd250bf",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 移动至俯视姿态\n",
    "mc.send_angles([-62.13, 8.96, -87.71, -14.41, 2.54, -16.34], 10)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 84,
   "id": "cfee14e0-1fc4-4c69-be6a-9b39cf49a7e4",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 运行`camera_check.py`，用鼠标点选图像中的某个点，获取像素坐标\n",
    "X_im, Y_im = 320, 240"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 85,
   "id": "22178096-dd19-47d9-9355-adbed0d6a175",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 手眼标定转换为机械臂坐标\n",
    "X_mc, Y_mc = eye2hand(X_im, Y_im)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 86,
   "id": "fb7a11fc-037b-472a-b4c6-30cf3481d012",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 控制机械臂移动到这个点，看是否准确\n",
    "mc.send_coords([X_mc, Y_mc, 100, -178.24, 1.68, -134.33], 20)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e962d3c8-9ecf-45db-8a82-13abd582fb9e",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
